import numpy as np
import pandas as pd
from scipy.optimize import least_squares
import cv2
import os
from io import StringIO
import re
import matplotlib.pyplot as plt
import tkinter as tk
from tkinter import ttk

# 文件路径设置
DATA_DIR = os.path.dirname(os.path.abspath(__file__))  # 获取当前脚本所在目录
CSV_FILE_PATH = os.path.join(DATA_DIR, '靶标数据_20250604_170953.csv')  # CSV文件路径
OUTPUT_DIR = os.path.join(DATA_DIR, 'output')  # 输出目录
LOG_FILE = os.path.join(OUTPUT_DIR, 'optimization_log.txt')  # 优化日志文件
RESULT_FILE = os.path.join(OUTPUT_DIR, 'optimization_result.txt')  # 优化结果文件
PLOT_FILE = os.path.join(OUTPUT_DIR, 'residuals_plot.png')  # 残差图文件

# 确保输出目录存在
os.makedirs(OUTPUT_DIR, exist_ok=True)

# 误差参数设置
PIXEL_ERROR_STD = 0  # 像素坐标标准差（像素）
TARGET_COORD_ERROR_STD = 0  # 靶标坐标系坐标标准差（mm）
TOTAL_STATION_ERROR_STD = 0  # 全站仪测量靶标原点标准差（mm）

# 优化参数设置
MAX_ITERATIONS = 1000  # 最大迭代次数

def euler_to_matrix(euler):
    """欧拉角转旋转矩阵，ZYX顺序，单位为度"""
    rx, ry, rz = np.radians(euler)
    Rx = np.array([
        [1, 0, 0],
        [0, np.cos(rx), -np.sin(rx)],
        [0, np.sin(rx), np.cos(rx)]
    ])
    Ry = np.array([
        [np.cos(ry), 0, np.sin(ry)],
        [0, 1, 0],
        [-np.sin(ry), 0, np.cos(ry)]
    ])
    Rz = np.array([
        [np.cos(rz), -np.sin(rz), 0],
        [np.sin(rz), np.cos(rz), 0],
        [0, 0, 1]
    ])
    # 修改为ZYX顺序，与生成数据程序保持一致
    return Rz @ Ry @ Rx

def read_simulation_data(file_path=CSV_FILE_PATH):
    """读取仿真数据CSV文件并打印信息"""
    # 获取当前脚本所在目录
    current_dir = os.path.dirname(os.path.abspath(__file__))
    full_path = os.path.join(current_dir, file_path)
    if not os.path.exists(full_path):
        print(f"错误：找不到文件 '{file_path}'")
        print(f"请确保文件位于以下目录：{current_dir}")
        return None, None, None

    # 读取文件内容
    with open(full_path, 'r', encoding='utf-8') as f:
        lines = f.readlines()

    # 初始化参数字典
    camera_params = {}
    camera_extrinsic = {}
    df = None
    current_section = None
    data_lines = []

    # 逐行处理文件
    for line in lines:
        line = line.strip()
        if not line:
            continue

        # 检查是否是新的部分
        if line.startswith('==='):
            if '相机参数' in line:
                current_section = 'camera_params'
            elif '相机外参' in line:
                current_section = 'camera_extrinsic'
            elif '靶标数据' in line:
                current_section = 'target_data'
            continue

        # 根据当前部分处理数据
        if current_section == 'camera_params':
            if ',' in line:
                parts = line.split(',')
                if len(parts) == 2:
                    key = parts[0].strip()
                    try:
                        value = float(parts[1].strip())
                        camera_params[key] = value
                    except ValueError:
                        continue

        elif current_section == 'camera_extrinsic':
            if ',' in line and ('相机位置' in line or '相机欧拉角' in line):
                parts = line.split(',')
                if len(parts) == 2:
                    key = parts[0].strip()
                    try:
                        value = float(parts[1].strip())
                        camera_extrinsic[key] = value
                    except ValueError:
                        continue

        elif current_section == 'target_data':
            data_lines.append(line)

    # 处理靶标数据
    if data_lines:
        df = pd.read_csv(StringIO('\n'.join(data_lines)))

    # 打印读取到的参数
    print("\n=== 读取到的相机参数 ===")
    for key, value in sorted(camera_params.items()):
        print(f"{key}: {value}")

    print("\n=== 读取到的相机外参 ===")
    for key, value in sorted(camera_extrinsic.items()):
        print(f"{key}: {value}")

    # 验证必要的参数
    required_params = ['X方向焦距(像素)', 'Y方向焦距(像素)', '主点X坐标(像素)', '主点Y坐标(像素)']
    required_extrinsic = ['相机位置X(mm)', '相机位置Y(mm)', '相机位置Z(mm)',
                        '相机欧拉角X(度)', '相机欧拉角Y(度)', '相机欧拉角Z(度)']

    missing_params = [param for param in required_params if param not in camera_params]
    if missing_params:
        print("\n错误：缺少以下相机参数：")
        for param in missing_params:
            print(f"  - {param}")
        return None, None, None

    missing_extrinsic = [param for param in required_extrinsic if param not in camera_extrinsic]
    if missing_extrinsic:
        print("\n错误：缺少以下相机外参：")
        for param in missing_extrinsic:
            print(f"  - {param}")
        return None, None, None

    if df is None or df.empty:
        print("错误：未找到有效的靶标数据")
        return None, None, None

    return df, camera_params, camera_extrinsic

# 读取数据获取真值
df, camera_params, camera_extrinsic = read_simulation_data(CSV_FILE_PATH)

if df is None or camera_params is None or camera_extrinsic is None:
    print("错误：无法从CSV文件读取参数")
    exit(1)

# 从CSV文件中读取的真值
fx_true = camera_params['X方向焦距(像素)']
fy_true = camera_params['Y方向焦距(像素)']
cx_true = camera_params['主点X坐标(像素)']
cy_true = camera_params['主点Y坐标(像素)']

# 从CSV文件中读取相机外参真值
T_cw_true = np.array([
    camera_extrinsic['相机位置X(mm)'],
    camera_extrinsic['相机位置Y(mm)'],
    camera_extrinsic['相机位置Z(mm)']
])

# 从欧拉角计算旋转矩阵
euler_true = np.array([
    camera_extrinsic['相机欧拉角X(度)'],
    camera_extrinsic['相机欧拉角Y(度)'],
    camera_extrinsic['相机欧拉角Z(度)']
])

R_cw_true = euler_to_matrix(euler_true)

# 计算相机相对于全站仪的变换（用于优化）
R_wc_true = R_cw_true.T
T_wc_true = -R_wc_true @ T_cw_true

# 使用真值作为初值
fx0 = fx_true
fy0 = fy_true
cx0 = cx_true
cy0 = cy_true
T_wc0 = T_wc_true

def matrix_to_euler(R):
    """旋转矩阵转欧拉角（ZYX，度）"""
    sy = np.sqrt(R[0,0]**2 + R[1,0]**2)
    singular = sy < 1e-6
    if not singular:
        x = np.arctan2(R[2,1], R[2,2])
        y = np.arctan2(-R[2,0], sy)
        z = np.arctan2(R[1,0], R[0,0])
    else:
        x = np.arctan2(-R[1,2], R[1,1])
        y = np.arctan2(-R[2,0], sy)
        z = 0
    return np.degrees([x, y, z])

def show_position_residuals_dialog(residuals, Otw_list, optimized_params=None, true_params=None):
    """显示每个位置的几何一致性残差对话框和参数对比"""
    # 创建主窗口
    root = tk.Tk()
    root.title("优化结果与残差分析")
    
    # 设置窗口大小和位置
    window_width = 1000
    window_height = 800
    screen_width = root.winfo_screenwidth()
    screen_height = root.winfo_screenheight()
    x = (screen_width - window_width) // 2
    y = (screen_height - window_height) // 2
    root.geometry(f"{window_width}x{window_height}+{x}+{y}")

    # 创建参数对比框架
    param_frame = ttk.LabelFrame(root, text="参数对比", padding=10)
    param_frame.pack(fill=tk.X, padx=5, pady=5)

    if optimized_params is not None and true_params is not None:
        # 相机内参对比
        fx_opt, fy_opt, cx_opt, cy_opt = optimized_params[:4]
        fx_true, fy_true, cx_true, cy_true = true_params[:4]
        
        # 相机外参对比
        euler_opt = optimized_params[4:7]
        T_wc_opt = optimized_params[7:10]
        euler_true = true_params[4:7]
        T_wc_true = true_params[7:10]

        # 创建参数对比表格
        param_tree = ttk.Treeview(param_frame, columns=("Parameter", "Optimized", "True", "Error"), show="headings", height=10)
        param_tree.heading("Parameter", text="参数")
        param_tree.heading("Optimized", text="优化值")
        param_tree.heading("True", text="真值")
        param_tree.heading("Error", text="误差")
        
        param_tree.column("Parameter", width=150)
        param_tree.column("Optimized", width=150)
        param_tree.column("True", width=150)
        param_tree.column("Error", width=150)

        # 添加内参对比
        param_tree.insert("", "end", values=("fx (像素)", f"{fx_opt:.2f}", f"{fx_true:.2f}", f"{fx_opt-fx_true:.2f}"))
        param_tree.insert("", "end", values=("fy (像素)", f"{fy_opt:.2f}", f"{fy_true:.2f}", f"{fy_opt-fy_true:.2f}"))
        param_tree.insert("", "end", values=("cx (像素)", f"{cx_opt:.2f}", f"{cx_true:.2f}", f"{cx_opt-cx_true:.2f}"))
        param_tree.insert("", "end", values=("cy (像素)", f"{cy_opt:.2f}", f"{cy_true:.2f}", f"{cy_opt-cy_true:.2f}"))

        # 添加外参对比
        param_tree.insert("", "end", values=("欧拉角X (度)", f"{euler_opt[0]:.2f}", f"{euler_true[0]:.2f}", f"{euler_opt[0]-euler_true[0]:.2f}"))
        param_tree.insert("", "end", values=("欧拉角Y (度)", f"{euler_opt[1]:.2f}", f"{euler_true[1]:.2f}", f"{euler_opt[1]-euler_true[1]:.2f}"))
        param_tree.insert("", "end", values=("欧拉角Z (度)", f"{euler_opt[2]:.2f}", f"{euler_true[2]:.2f}", f"{euler_opt[2]-euler_true[2]:.2f}"))
        param_tree.insert("", "end", values=("T_wc X (mm)", f"{T_wc_opt[0]:.2f}", f"{T_wc_true[0]:.2f}", f"{T_wc_opt[0]-T_wc_true[0]:.2f}"))
        param_tree.insert("", "end", values=("T_wc Y (mm)", f"{T_wc_opt[1]:.2f}", f"{T_wc_true[1]:.2f}", f"{T_wc_opt[1]-T_wc_true[1]:.2f}"))
        param_tree.insert("", "end", values=("T_wc Z (mm)", f"{T_wc_opt[2]:.2f}", f"{T_wc_true[2]:.2f}", f"{T_wc_opt[2]-T_wc_true[2]:.2f}"))

        param_tree.pack(fill=tk.X, pady=5)

    # 创建残差框架
    residual_frame = ttk.LabelFrame(root, text="位置残差", padding=10)
    residual_frame.pack(fill=tk.BOTH, expand=True, padx=5, pady=5)

    # 创建残差表格
    tree = ttk.Treeview(residual_frame, columns=("Position", "X", "Y", "Z", "P1P3_Dist", "Norm"), show="headings")
    
    # 设置列标题
    tree.heading("Position", text="靶标位置")
    tree.heading("X", text="X方向残差(mm)")
    tree.heading("Y", text="Y方向残差(mm)")
    tree.heading("Z", text="Z方向残差(mm)")
    tree.heading("P1P3_Dist", text="P1P3距离残差(mm)")
    tree.heading("Norm", text="残差范数(mm)")
    
    # 设置列宽
    tree.column("Position", width=150)
    tree.column("X", width=150)
    tree.column("Y", width=150)
    tree.column("Z", width=150)
    tree.column("P1P3_Dist", width=150)
    tree.column("Norm", width=150)

    # 添加滚动条
    scrollbar = ttk.Scrollbar(residual_frame, orient="vertical", command=tree.yview)
    tree.configure(yscrollcommand=scrollbar.set)

    # 计算每个位置的残差
    n_positions = len(Otw_list)
    for i in range(n_positions):
        # 获取位置残差（每个位置有4个残差：X,Y,Z和P1P3距离）
        x_res = residuals[i*4]
        y_res = residuals[i*4 + 1]
        z_res = residuals[i*4 + 2]
        p1p3_res = residuals[i*4 + 3]
        
        # 计算该位置的总残差范数
        norm = np.sqrt(x_res**2 + y_res**2 + z_res**2 + p1p3_res**2)
        
        # 获取靶标位置坐标
        Otw = Otw_list[i]
        position_str = f"({Otw[0]:.1f}, {Otw[1]:.1f}, {Otw[2]:.1f})"
        
        tree.insert("", "end", values=(
            position_str,
            f"{x_res:.3f}",
            f"{y_res:.3f}",
            f"{z_res:.3f}",
            f"{p1p3_res:.3f}",
            f"{norm:.3f}"
        ))

    # 计算并显示统计信息
    x_residuals = residuals[0::4]
    y_residuals = residuals[1::4]
    z_residuals = residuals[2::4]
    p1p3_residuals = residuals[3::4]
    
    stats_text = f"""
统计信息：
X方向: 均值={np.mean(x_residuals):.3f}mm, 标准差={np.std(x_residuals):.3f}mm
Y方向: 均值={np.mean(y_residuals):.3f}mm, 标准差={np.std(y_residuals):.3f}mm
Z方向: 均值={np.mean(z_residuals):.3f}mm, 标准差={np.std(z_residuals):.3f}mm
P1P3距离: 均值={np.mean(p1p3_residuals):.3f}mm, 标准差={np.std(p1p3_residuals):.3f}mm
总体RMS残差: {np.sqrt(np.mean(np.square(residuals))):.3f}mm
"""
    stats_label = ttk.Label(residual_frame, text=stats_text, justify=tk.LEFT)
    
    # 布局
    tree.pack(side=tk.LEFT, fill=tk.BOTH, expand=True, pady=5)
    scrollbar.pack(side=tk.RIGHT, fill=tk.Y, pady=5)
    stats_label.pack(pady=10, padx=5, anchor=tk.W)
    
    # 添加关闭按钮
    close_button = ttk.Button(root, text="关闭", command=root.destroy)
    close_button.pack(pady=10)
    
    # 运行对话框
    root.mainloop()

def compute_geometric_consistency_error(params, Otw_list, image_points_list, object_points):
    """计算每个位置的几何一致性误差"""
    fx, fy, cx, cy = params[:4]
    T_wc = params[4:7]
    K = np.array([[fx, 0, cx],
                  [0, fy, cy],
                  [0,  0,  1]])
    
    errors = []
    for i, (Otw, img_pts) in enumerate(zip(Otw_list, image_points_list)):
        # 使用PnP求解靶标在相机坐标系下的位置
        _, rvec, tvec = cv2.solvePnP(object_points, img_pts, K, None, flags=cv2.SOLVEPNP_ITERATIVE)
        R_tc = cv2.Rodrigues(rvec)[0]
        T_tc = tvec.flatten()
        
        # 计算1号点和3号点在相机坐标系下的位置
        P1_c = R_tc @ object_points[0] + T_tc
        P3_c = R_tc @ object_points[2] + T_tc
        
        # 从CSV文件中读取全站仪测量的坐标
        P1_str = df.iloc[i]['靶标原点在全站仪坐标系下的坐标Ot_w'].strip('[]').split(',')
        P3_str = df.iloc[i]['靶标三号点在全站仪坐标系下的坐标P3_w'].strip('[]').split(',')
        
        P1_w_measured = np.array([float(x.strip()) for x in P1_str])
        P3_w_measured = np.array([float(x.strip()) for x in P3_str])
        
        # 使用优化后的外参将点转换到全站仪坐标系
        # R_wc_true是从世界坐标系到相机坐标系的变换矩阵
        # T_wc是相机坐标系原点在世界坐标系中的位置
        P1_w_estimated = R_wc_true.T @ (P1_c - T_wc)
        P3_w_estimated = R_wc_true.T @ (P3_c - T_wc)
        
        # 计算位置误差
        P1_error = P1_w_estimated - P1_w_measured
        P3_error = P3_w_estimated - P3_w_measured
        
        # 计算距离误差
        dist_estimated = np.linalg.norm(P3_w_estimated - P1_w_estimated)
        dist_measured = np.linalg.norm(P3_w_measured - P1_w_measured)
        dist_error = dist_estimated - dist_measured
        
        # 存储误差
        errors.append({
            'P1_error': P1_error,
            'P3_error': P3_error,
            'dist_error': dist_error,
            'P1_w_estimated': P1_w_estimated,
            'P3_w_estimated': P3_w_estimated,
            'P1_w_measured': P1_w_measured,
            'P3_w_measured': P3_w_measured
        })
    
    return errors

def plot_residuals(errors, Otw_list):
    """绘制几何一致性误差图"""
    n_positions = len(errors)
    positions = range(1, n_positions + 1)
    
    # 提取误差数据
    x_errors = [error['P1_error'][0] for error in errors]
    y_errors = [error['P1_error'][1] for error in errors]
    z_errors = [error['P1_error'][2] for error in errors]
    
    # 创建图形
    plt.figure(figsize=(12, 6))
    
    # 绘制误差
    plt.plot(positions, x_errors, 'r-o', label='X Error', linewidth=2, markersize=8)
    plt.plot(positions, y_errors, 'g-o', label='Y Error', linewidth=2, markersize=8)
    plt.plot(positions, z_errors, 'b-o', label='Z Error', linewidth=2, markersize=8)
    
    # 设置图表属性
    plt.title('Geometric Consistency Errors', fontsize=14)
    plt.xlabel('Target Position Index', fontsize=12)
    plt.ylabel('Error (mm)', fontsize=12)
    plt.grid(True, linestyle='--', alpha=0.7)
    plt.legend(fontsize=10)
    
    # 设置x轴刻度
    plt.xticks(positions)
    
    # 调整布局
    plt.tight_layout()
    
    # 打印误差统计信息
    print("\n=== Geometric Consistency Error Statistics ===")
    print(f"X Error: mean = {np.mean(x_errors):.3f}mm, std = {np.std(x_errors):.3f}mm")
    print(f"Y Error: mean = {np.mean(y_errors):.3f}mm, std = {np.std(y_errors):.3f}mm")
    print(f"Z Error: mean = {np.mean(z_errors):.3f}mm, std = {np.std(z_errors):.3f}mm")

def optimize_parameters(x0, Otw_list, image_points_list, object_points):
    """使用LM算法进行参数优化，只优化内参和平移向量"""
    iter_log = []
    
    # 定义参数边界
    bounds = (
        # 下界
        [
            fx_true - 100,  # fx最小值，减小范围
            fy_true - 100,  # fy最小值，减小范围
            cx_true - 20,   # cx最小值，减小范围
            cy_true - 20,   # cy最小值，减小范围
            T_wc_true[0] - 50,  # T_wc_x最小值
            T_wc_true[1] - 50,  # T_wc_y最小值
            T_wc_true[2] - 50   # T_wc_z最小值
        ],
        # 上界
        [
            fx_true + 100,  # fx最大值，减小范围
            fy_true + 100,  # fy最大值，减小范围
            cx_true + 20,   # cx最大值，减小范围
            cy_true + 20,   # cy最大值，减小范围
            T_wc_true[0] + 50,  # T_wc_x最大值
            T_wc_true[1] + 50,  # T_wc_y最大值
            T_wc_true[2] + 50   # T_wc_z最大值
        ]
    )
    
    # 使用LM算法进行优化，添加边界约束
    result = least_squares(
        lambda x: residual_func(x, Otw_list, image_points_list, object_points, iter_log),
        x0,
        method='trf',  # 使用trf方法支持边界约束
        ftol=1e-12,
        xtol=1e-12,
        max_nfev=MAX_ITERATIONS,
        bounds=bounds,
        verbose=2
    )
    
    return result, iter_log

def residual_func(params, Otw_list, image_points_list, object_points, iter_log=None):
    """计算残差函数，只保留几何一致性约束"""
    fx, fy, cx, cy = params[:4]
    T_wc = params[4:7]
    K = np.array([[fx, 0, cx],
                  [0, fy, cy],
                  [0,  0,  1]])
    
    # 预分配残差数组
    n_positions = len(Otw_list)
    n_geometric_residuals = 6  # 每个位置6个几何残差（P1和P3的x,y,z）
    total_residuals = n_positions * n_geometric_residuals
    res = np.zeros(total_residuals)
    
    # 计算几何一致性残差
    for i, (Otw, img_pts) in enumerate(zip(Otw_list, image_points_list)):
        # PnP解靶标原点在相机坐标系下的位置
        _, rvec, tvec = cv2.solvePnP(object_points, img_pts, K, None, flags=cv2.SOLVEPNP_ITERATIVE)
        R_tc = cv2.Rodrigues(rvec)[0]
        T_tc = tvec.flatten()
        
        # 计算1号点和3号点在相机坐标系下的位置（通过PnP）
        P1_c_pnp = R_tc @ object_points[0] + T_tc
        P3_c_pnp = R_tc @ object_points[2] + T_tc
        
        # 从CSV文件中读取全站仪测量的1号点和3号点坐标
        P1_str = df.iloc[i]['靶标原点在全站仪坐标系下的坐标Ot_w'].strip('[]').split(',')
        P3_str = df.iloc[i]['靶标三号点在全站仪坐标系下的坐标P3_w'].strip('[]').split(',')
        
        P1_w = np.array([float(x.strip()) for x in P1_str])
        P3_w = np.array([float(x.strip()) for x in P3_str])
        
        # 通过外参将全站仪测量的点变换到相机坐标系
        # R_wc_true是从世界坐标系到相机坐标系的变换矩阵
        # T_wc是相机坐标系原点在世界坐标系中的位置
        P1_c_ext = R_wc_true @ P1_w + T_wc
        P3_c_ext = R_wc_true @ P3_w + T_wc
        
        # 计算位置误差（使用相对误差）
        idx = i * n_geometric_residuals
        # 归一化误差，使不同尺度的误差具有可比性
        res[idx:idx+3] = (P1_c_pnp - P1_c_ext) / np.linalg.norm(P1_c_pnp)  # P1的x,y,z误差
        res[idx+3:idx+6] = (P3_c_pnp - P3_c_ext) / np.linalg.norm(P3_c_pnp)  # P3的x,y,z误差
    
    # 记录每次迭代的参数和残差
    if iter_log is not None:
        iter_log.append({
            'fx': fx, 'fy': fy, 'cx': cx, 'cy': cy,
            'T_wc': T_wc.copy(),
            'residual_norm': np.linalg.norm(res),
            'residuals': res.copy(),
            'Otw_list': Otw_list
        })
    
    return res

def parse_pixel_coords(pixel_str):
    """解析像素坐标字符串"""
    coords = re.findall(r'\(([\d\.\-]+),\s*([\d\.\-]+)\)', pixel_str)
    coords = np.array([[float(x), float(y)] for x, y in coords], dtype=np.float32)
    return coords

def parse_vector(vec_str):
    """解析向量字符串"""
    nums = vec_str.strip('[]').split(',')
    vec = np.array([float(x) for x in nums])
    return vec

def main():
    # 读取CSV文件
    if not os.path.exists(CSV_FILE_PATH):
        print(f"Error: File '{CSV_FILE_PATH}' not found")
        return

    with open(CSV_FILE_PATH, 'r', encoding='utf-8') as f:
        lines = f.readlines()
    
    # 找到靶标数据部分
    data_start, data_end = None, None
    for i, line in enumerate(lines):
        if "=== 靶标数据 ===" in line:
            data_start = i + 1
        if data_start is not None and (line.strip() == "" or line.startswith("===")) and i > data_start:
            data_end = i
            break
    if data_start is None:
        print("Error: Target data section not found")
        return
    if data_end is None:
        data_end = len(lines)
    data_lines = lines[data_start:data_end]
    df = pd.read_csv(StringIO(''.join(data_lines)))

    # 组装优化所需数据
    Otw_list = []
    image_points_list = []
    for _, row in df.iterrows():
        img_pts = parse_pixel_coords(row['靶标点像素坐标'])
        image_points_list.append(img_pts)
        Otw = parse_vector(row['靶标原点在全站仪坐标系下的坐标Ot_w'])
        Otw_list.append(Otw)

    # 靶标三维点（单位mm）
    object_points = np.array([
        [0,   0,   0],    # P1
        [500, 0,   0],    # P2
        [500, 500, 0],    # P3
        [0,   500, 0],    # P4
        [-250,   250,   -300],  # P5
        [750, 250, -300],  # P6
    ], dtype=np.float32)

    # 初值
    x0 = np.array([fx0, fy0, cx0, cy0, *T_wc0])

    # 使用LM算法进行优化
    result, iter_log = optimize_parameters(x0, Otw_list, image_points_list, object_points)
    
    # 获取优化结果
    fx, fy, cx, cy = result.x[:4]
    T_wc = result.x[4:7]
    
    # 计算相机相对于全站仪的外参
    R_cw = R_cw_true.T
    T_cw = -R_cw @ T_wc
    
    # 打印优化结果和真值对比
    print("\n=== Optimization Results ===")
    print("\nCamera Intrinsics:")
    print(f"fx = {fx:.2f} (true: {fx_true:.2f}, error: {fx-fx_true:.2f})")
    print(f"fy = {fy:.2f} (true: {fy_true:.2f}, error: {fy-fy_true:.2f})")
    print(f"cx = {cx:.2f} (true: {cx_true:.2f}, error: {cx-cx_true:.2f})")
    print(f"cy = {cy:.2f} (true: {cy_true:.2f}, error: {cy-cy_true:.2f})")
    
    print("\nCamera Extrinsics (relative to total station):")
    print("Rotation Matrix R_cw (true value):")
    print("Used:")
    for row in R_cw:
        print(f"  [{row[0]:.6f}, {row[1]:.6f}, {row[2]:.6f}]")
    print("\nTrue:")
    for row in R_cw_true.T:  # 使用R_cw_true.T作为真值
        print(f"  [{row[0]:.6f}, {row[1]:.6f}, {row[2]:.6f}]")
    
    print("\nTranslation Vector T_cw:")
    print(f"Optimized: [{T_cw[0]:.2f}, {T_cw[1]:.2f}, {T_cw[2]:.2f}]")
    print(f"True: [{T_cw_true[0]:.2f}, {T_cw_true[1]:.2f}, {T_cw_true[2]:.2f}]")
    
    # 计算误差
    print("\n=== Error Analysis ===")
    print(f"Intrinsic Errors:")
    print(f"  fx: {fx-fx_true:.2f} pixels")
    print(f"  fy: {fy-fy_true:.2f} pixels")
    print(f"  cx: {cx-cx_true:.2f} pixels")
    print(f"  cy: {cy-cy_true:.2f} pixels")
    
    # 计算旋转矩阵误差
    R_error = np.linalg.norm(R_cw - R_cw_true, 'fro')
    print(f"Extrinsic Rotation Error: {R_error:.6f}")
    
    # 计算平移向量误差
    T_error = np.linalg.norm(T_cw - T_cw_true)
    print(f"Extrinsic Translation Error: {T_error:.2f} mm")
    
    # 计算几何一致性误差
    errors = compute_geometric_consistency_error(result.x, Otw_list, image_points_list, object_points)
    
    # 绘制误差图
    plot_residuals(errors, Otw_list)
    plt.show()

if __name__ == "__main__":
    main()
